/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file template.cxx
	\brief class implemenation for template

                    \author Jack Elston
             $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <iostream>
#include <stdio.h>
#include <math.h>

#include "stack.h"
#include "utilities.h"
#include "color.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
extern bool verbose;
/*<-End Global Variables-->*/

StackingController::StackingController(String name) : ThreadedPipe(name)
{
	last_control_loop.stamp();
	cout << GREEN << "---------->>>>>      " YELLOW "STACK" GREEN "       <<<<<----------" << ATTROFF << endl;
}

void StackingController::update() 
{
	int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

	FD_ZERO(&readFDs);	               		// zero read fd set
	FD_ZERO(&writeFDs);	               		// zero write fd set
	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

	fifo_FDs[INBOARD_RX] > fifo_FDs[OUTBOARD_RX] ? maxFD = fifo_FDs[INBOARD_RX] : maxFD = fifo_FDs[OUTBOARD_RX];
	if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
	if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	Client * me = shmPtr->clients.getClient(0);

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("StackingController::update() select");
				execRate = -1;
			} else
				cout << getName() << "::update() - select interrupted" << endl;
		}
	}
	else {
		if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
		{
			int n = 0;
			n=readFifo(OUTBOARD_RX);
			if(n>0) {
				uint16_t length = CMD_SIZE(fifo_msg.data);
				if( length != fifo_msg.size) {
					cout << getName() << "::update() - error, length of command != length of buffer" << endl
						<< "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

					if( length < 1000 && length <= fifo_msg.size ) {
						if( C_calcCheckSum((uint8_t *)buf) == 0 ) 
							cout << "\tCMD: is valid" << endl;
						else
							cout << "\tCMD: is NOT valid" << endl;
					}

					return;
				}

				Command cmd(fifo_msg.data);

				if( !cmd.isValid() )
					cout << getName() << "::update() - error, invalid command" << endl << cmd ;
				else {
					switch(cmd.cmd() ) {
						case CMD_RETURN: {
							;
						} break;
						default:
							writeFifo(INBOARD_TX);
							break;
					}
				}
			}
		}

		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
			int n = 0;
			n=readFifo(INBOARD_RX);
			if(n>0) {
				uint16_t length = CMD_SIZE(fifo_msg.data);
				if( length != fifo_msg.size) {
					cout << getName() << "::update() - error, length of command != length of buffer" << endl
						<< "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

					if( length < 1000 && length <= fifo_msg.size ){
						if( C_calcCheckSum((uint8_t *)buf) == 0 ) 
							cout << "\tCMD: is valid" << endl;
						else
							cout << "\tCMD: is NOT valid" << endl;
					}

					return;
				}

				Command cmd(fifo_msg.data);

				if( !cmd.isValid() )
					cout << getName() << "::update() - error, invalid command" << endl << cmd ;
				else {
					Client * a_client;
					a_client = shmPtr->clients.getClient(fifo_msg.src);
					if(a_client != NULL) {

						uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);

						switch(cmd.cmd() ) {
							case CMD_FORMATION: {
									int num_vehicles = cmd.size()/sizeof(VehicleInfo_t);

									if(num_vehicles == 0) {
										CMD_CMD(fifo_msg.data)  = CMD_RETURN;
										CMD_NUM(fifo_msg.data)  = 0;
										CMD_CSUM(fifo_msg.data) = 0;
										CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

										fifo_msg.size = CMD_SIZE(fifo_msg.data);
										fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr;
										fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
										writeFifo(OUTBOARD_TX);
									}

									me->telemetry.navigation.OBmode = OB_OFF; 
									me->telemetry.navigation.OBtype = OB_STACK; 
									formation_vehicles.clear();
									int sub_sum=0;
									if( num_vehicles == 0) {
										me->telemetry.navigation.OBmode = OB_INIT; 
										for(int j=0;j<num_vehicles;j++){
											if((a_client = shmPtr->clients.getClient(((VehicleInfo_t*)dataPtr)[j].id)) != NULL) {
												if(formation_vehicles.add(a_client)) {
													last_track = 0;
													cout << "requresting TELEM stream from " << a_client->name << endl;
													subscribe(CMD_TELEM, a_client, true, INBOARD_TX);
													sub_sum++;
												}
											}
										}
										if( sub_sum > 0 ) {
											ACK(INBOARD_TX);
											cout << "---------->>>>>    IN STACK FORMATION    <<<<<----------" << endl;
										} else {
											me->telemetry.navigation.OBmode = OB_ERROR; 
											NACK(INBOARD_TX);
											formation_vehicles.clear();
										}
									} else
										ACK(INBOARD_TX);

								} break;
							case CMD_ACK: {
									cout << "CMD_ACK from " << a_client->name << endl;
								} break;
							case CMD_FAIL: {
									cout << "CMD_FAIL from " << a_client->name << endl;
								} break;
							case CMD_TELEM: {
									//cout << "CMD_TELEM from " << a_client->name << endl;
									Telemetry_t * telemetry  = (Telemetry_t*)dataPtr;
									Client * formation_client = formation_vehicles.getClient(a_client->name);
									if(formation_client != NULL) {
										formation_client->telemetry.gps.pos.lat=telemetry->gps.pos.lat;
										formation_client->telemetry.gps.pos.lon=telemetry->gps.pos.lon;
										formation_client->telemetry.gps.track=telemetry->gps.track;
										formation_client->telemetry.gps.sog=telemetry->gps.sog;
									}
								} break;
							case CMD_GPS_TELEM: {
									//cout << "CMD_GPS_TELEM from " << a_client->name << endl;
									GPSTelem_t * telemetry  = (GPSTelem_t*)dataPtr;
									Client * formation_client = formation_vehicles.getClient(a_client->name);
									if(formation_client != NULL) {
										formation_client->telemetry.gps.pos.lat=telemetry->pos.lat;
										formation_client->telemetry.gps.pos.lon=telemetry->pos.lon;
										formation_client->telemetry.gps.track=telemetry->track;
										formation_client->telemetry.gps.sog=telemetry->sog;
									}
								} break;
							case CMD_FP: {
									writeFifo(OUTBOARD_TX);

									if(formation_vehicles.getNumClients() != 0) {

										CMD_CMD(fifo_msg.data)  = CMD_RETURN;
										CMD_NUM(fifo_msg.data)  = 0;
										CMD_CSUM(fifo_msg.data) = 0;
										CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

										fifo_msg.size = CMD_SIZE(fifo_msg.data);
										fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr;
										fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
										writeFifo(OUTBOARD_TX);
									}

									me->telemetry.navigation.OBmode = OB_OFF; 
									me->telemetry.navigation.OBtype = OB_NONE; 
									formation_vehicles.clear();

								} break;
							default:
								writeFifo(OUTBOARD_TX);
								break;
						}
					}
				}
			}
		}
	}

	// time stamps and control update rate
	currentStamp.stamp();
	if((currentStamp - last_control_loop ) > (STACKING_LOOP_PERIOD * SEC2MICRO) ) {
		bool got_info = true;
		for(int i=0; i< formation_vehicles.getNumClients(); i ++)
			if(((formation_vehicles.getClient(i))->telemetry.gps.pos.lat) == 0.0) {
				cout << "requresting TELEM stream from " << (formation_vehicles.getClient(i))->name << endl;
				subscribe(CMD_TELEM, formation_vehicles.getClient(i), true, INBOARD_TX);
				got_info = false;
			}
		if((formation_vehicles.getNumClients() > 0) && got_info) { followLeader(); } 
		last_control_loop.stamp();
	}
}

void StackingController::followLeader() 
{
	GPSLatLon_t projected_location;
	projected_location.lat=0.0;
	projected_location.lon=0.0;

	Client * leader = formation_vehicles.getClient(0);

	float d_track = (last_track - (leader->telemetry.gps.track));
	while (d_track > 360)
		d_track = d_track-360;
	while (d_track < 0)
		d_track = d_track+360;

	float pred_track = (leader->telemetry.gps.track - 0.5 * (d_track)); 
	while (pred_track > 360)
		pred_track = pred_track-360;
	while (pred_track < 0)
		pred_track = pred_track+360;

	cout << "track " << leader->telemetry.gps.track << " prediction " << pred_track << endl;

	last_track = leader->telemetry.gps.track;
	float v_y = cos(pred_track*M_PI/180)*leader->telemetry.gps.sog;
	float v_x = sin(pred_track*M_PI/180)*leader->telemetry.gps.sog;

	//cout << "d_lat " << v_x << " d_lon " << v_y << endl;

	projected_location.lat = leader->telemetry.gps.pos.lat + M_TO_LAT(v_y);
	projected_location.lon = leader->telemetry.gps.pos.lon - M_TO_LON(v_x,(leader->telemetry.gps.pos.lat));

	cout << "acutal  " << leader->telemetry.gps.pos.lat << "," << leader->telemetry.gps.pos.lon << " projected " << projected_location.lat << "," << projected_location.lon << endl;

	Client * me = shmPtr->clients.getClient(0);

	float psides = 0;
	float vdes = 0;

	float d_lat = LAT_TO_M(me->telemetry.gps.pos.lat - projected_location.lat);
	float d_lon = LON_TO_M(me->telemetry.gps.pos.lon - projected_location.lon,me->telemetry.gps.pos.lat);

	float r = sqrt(d_lat*d_lat + d_lon*d_lon);
	float target_heading = atan2(d_lon, d_lat)*180/M_PI;
	//cout << "target heading " << target_heading;
	target_heading = 180.0+target_heading;
	//cout << " : " << target_heading;
	while (target_heading > 360.0)
		target_heading = target_heading-360.0;
	while (target_heading < 0.0)
		target_heading = target_heading+360.0;

	//cout << " : " << target_heading << endl;
	psides = target_heading*M_PI/180; 
	vdes =  0.02*r + leader->telemetry.gps.sog;

	me->telemetry.navigation.OBmode = OB_TURNRATE; 

	APRates_t * ap_rates = (APRates_t*)CMD_DATAPTR(fifo_msg.data);
	//ap_rates->heading=psides; //FIXME -- need to use turn rate not heading
	ap_rates->velocity=vdes;

	CMD_CMD(fifo_msg.data) = CMD_AP_RATES;
	CMD_NUM(fifo_msg.data) = sizeof(APRates_t);
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	writeFifo(OUTBOARD_TX);
}
